/*LICENSE_BEGIN
 * ******************************************************************
 * Copyright 2006 by the Dainamite Robocup-Team at the
 * DAI-Labor, TU-Berlin (www.dai-labor.de)
 * 
 * Members of Dainamite are:
 * Holger Endert
 * Robert Wetzker
 * Thomas Karbe
 * Axel Heßler
 * Philippe Bütter
 * Felix Brossmann
 * 
 * Licensed under the GNU GPL (General Public License) Free Software 
 * License, Version 2. You may use this file only in compliance with 
 * the License. You may obtain a copy of the License at:
 * 
 * http://www.fsf.org/licensing/licenses/gpl.txt
 * ******************************************************************
LICENSE_END*/
package robocup.component.worldmodel.particlefilter;

import java.util.Random;

import robocup.component.geometry.Vektor;
import robocup.component.infotypes.FlagInfo;

public class ParticleFilter {

	private Vektor particles[];

	private Vektor particles2[];

	private int validParticles = 0;

	private Random random = new Random(System.currentTimeMillis());

	private Region regionHelper = null;

	private Vektor vektorHelper = new Vektor();

	/**
	 * A class implementing the particle-filter as described in the UvA Master
	 * Thesis. The filter is used for determin the players position.
   * 
   * This Particle Filter is more accurate but less efficient then the other.
   * It is therefore only used, when the ball is near as otherwise the accuracy
   * is of less importance.
	 */
	public ParticleFilter() {
		super();
		this.particles = new Vektor[100];
		this.particles2 = new Vektor[100];
		for (int i = 0; i < 100; i++) {
			this.particles[i] = new Vektor();
			this.particles2[i] = new Vektor();
		}
		this.validParticles = 0;
	}

	public void resetPosition(Vektor pos) {
		if (pos == null) {
			this.validParticles = 0;
		} else {
			this.validParticles = 1;
			this.particles[0].x = pos.x;
			this.particles[0].y = pos.y;
		}
	}

	private boolean afterTrans = false;

	private long transCount = 0;

	private long nullpartCount = 0;

	public void addFlag(FlagInfo fi, double headAngle, Vektor flagpos) {
		if (this.regionHelper == null)
			this.regionHelper = new Region(fi, headAngle, flagpos);
		else
			this.regionHelper.reinitRegionFlag(fi, headAngle, flagpos);

		if (this.validParticles != 0) {
			// compute mean
			Vektor mean = this.vektorHelper;
			mean.x = 0;
			mean.y = 0;
			for (int i = 0; i < this.validParticles; i++) {
				mean.x += this.particles[i].x;
				mean.y += this.particles[i].y;
			}
			mean.x /= (double) this.validParticles;
			mean.y /= (double) this.validParticles;
			// compute standard deviation
			double dev = 0;
			for (int i = 0; i < this.validParticles; i++) {
				double deltax = this.particles[i].x - mean.x;
				double deltay = this.particles[i].y - mean.y;
				dev += (deltax * deltax + deltay * deltay);
			}
			dev /= (double) this.validParticles;
			dev = Math.sqrt(dev) / 12;
			dev *= Math.pow(1.0 / (double) this.validParticles, 1.0 / 6.0);
			// resample
			for (int i = this.validParticles; i < this.particles.length; i++) {
				Vektor chosen = this.particles[this.random
						.nextInt(this.validParticles)];
				this.particles[i].x = chosen.x
						+ (this.random.nextGaussian() * dev);
				this.particles[i].y = chosen.y
						+ (this.random.nextGaussian() * dev);
			}
			this.validParticles = this.particles.length;
			int lastValidPlusOne = 0;
			for (int i = 0; i < this.validParticles; i++) {
				Vektor helper = null;
				if (this.regionHelper.insideRegion(this.particles[i]) == 1.0) {
					if (lastValidPlusOne != i) {
						helper = this.particles[i];
						this.particles[i] = this.particles[lastValidPlusOne];
						this.particles[lastValidPlusOne] = helper;
						// lastValidPlusOne++;
					}
					lastValidPlusOne++;
				}
			}
			this.validParticles = lastValidPlusOne;
		}
		if (this.afterTrans) {
			this.afterTrans = false;
			this.transCount++;
			if (this.validParticles == 0)
				this.nullpartCount++;
		}
		if (this.validParticles == 0) {
			for (int i = 0; i < this.particles.length; i++) {
				this.regionHelper.makeRandomInsideRegion(this.particles[i]);
			}
			this.validParticles = this.particles.length;
		}
	}

	public Vektor getPosition() {
		if (this.validParticles == 0) {
			return null;
		}
		Vektor mean = this.vektorHelper;
		mean.x = 0;
		mean.y = 0;
		for (int i = 0; i < this.validParticles; i++) {
			mean.x += this.particles[i].x;
			mean.y += this.particles[i].y;
		}
		mean.x /= (double) this.validParticles;
		mean.y /= (double) this.validParticles;
		return mean.cloned();
	}

	public void translatePosition(Vektor speed) {

		this.afterTrans = true;

		if (this.validParticles == 0)
			return;
		this.regionHelper.reinitRegionSpeed(speed, this.particles[0]);
		for (int i = 0; i < this.validParticles; i++) {
			this.regionHelper.setOrigin(this.particles[i]);
			this.regionHelper.makeRandomInsideRegion(this.particles2[i]);
		}
		Vektor swaphelper[] = this.particles;
		this.particles = this.particles2;
		this.particles2 = swaphelper;
	}

}
